Creating IK Solver for Kuka KR210

By now you should have performed the Kinematic Analysis described so far and reached the point where you have equations for six joint variables. Now you will learn how to turn your Kinematic Analysis into a python program to perform Inverse Kinematics.

In the project folder you can find a template file named IK_server.py under:

~/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/scripts/

This file implements a ROS Server Node that caters to the CalculateIK.srv service:

# CalculateIK.srv file
# request
geometry_msgs/Pose[] poses
---
# response
trajectory_msgs/JointTrajectoryPoint[] points

Essentially IK_server.py receives end-effector poses from the pick and place simulator and is responsible to perform Inverse Kinematics, providing a response to the simulator with calculated joint variable values (six joint angles in our case).

Let us quickly walk through different parts of this template:

# import modules
import rospy
import tf
from kuka_arm.srv import *
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from geometry_msgs.msg import Pose
from mpmath import *
from sympy import *

We first import the python library for ros, followed by tf package which we described earlier. Since this is a server node we import the service messages from kuka_arm package.

When set in test mode, we will receive an IK request from the simulator with end-effector poses using the Pose msg from geometry_msgs.

JointTrajectoryPoint from trajectory_msgs will be used to pack individual joint variables (obtained after completing IK) into a single message (more details here).

In addition we also import sympy and mpmath modules for facilitating symbolic math in python.

def IK_server():
    # initialize node and create calculate_ik service
    rospy.init_node('IK_server')
    s = rospy.Service('calculate_ik', CalculateIK, handle_calculate_IK)
    print "Ready to receive an IK request"
    rospy.spin()

This function initializes our IK_server node and creates a CalculateIK type service with the name calculate_ik.

The function handle_calculate_IK serves as a callback function for when a request for this service type is received.

rospy.spin() as we learned before, simply keeps your node from exiting until it has been explicitly shutdown.

def handle_calculate_IK(req):
    rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
    if len(req.poses) < 1:
        print "No valid poses received"
        return -1
    else:
        ### Your FK code here
        #
        #
        ###

        # Initialize service response
        joint_trajectory_list = []
        for x in xrange(0, len(req.poses)):
            # IK code starts here
            joint_trajectory_point = JointTrajectoryPoint()
            ### Your IK code here
            #
            #
            ###

            # Populate response for the IK request
            joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6]
            joint_trajectory_list.append(joint_trajectory_point)

        rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list))
        return CalculateIKResponse(joint_trajectory_list)

This is the function that handles a CalculateIK service request and this is where you will be writing all of your IK code.

First we print out the number of end-effector poses received from the request and check whether the request was empty, in which case we print an appropriate message and return from this function with an error code.

If the request has a valid number of end-effector poses, we initialize an empty list named joint_trajectory_list which will contain the joint angle values that your code calculates.

Finally we start a loop to go through all the end-effector poses received from the request. joint_trajectory_point is an instance of JointTrajectoryPoint msg which contains joint angle positions, velocities, accelerations, and efforts but we will only be using position field for our code for a specific end-effector position.

After your code has calculated individual joint angles for a given eef pose, we populate the joint_trajectory_list and send back as a response to the simulator.

What should your code contain?

As described above, your implementation for IK_server.py can be split into two parts.

First step is to obtain individual transformation matrices from your calculated DH Parameters table.

#######################################
# Create symbols
#
#   
# Create Modified DH parameters
#
#            
# Define Modified DH Transformation matrix
#
#
# Create individual transformation matrices
#
#
# Extract rotation matrices from the transformation matrices
#
#
##################################################

The next step focuses on calculating the joint angles based on the position and orientation of the end-effector

##################################################            
# Extract end-effector position and orientation from request
#
#     
# Compensate for rotation discrepancy between DH parameters and Gazebo
#
#
# Calculate joint angles using Geometric IK method
#
#
##################################################

These have one-one correspondence with your kinematic analysis and hence should be conceptually clear.